////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                        //
//  ----------------------------- Ebrahim Foulaadvand, 03 August 2012 ----------------------------------- //
//                                                                                                        //
// The programme "DrivenDampedPedulum" evaluates the temporal evolution of a damped driven pendulum       //
// by two algorithms Euler-Richardson (ER) and 2nd order Runge-Kutta (RK2).                               //
//                                                                                                        //
//                                                                                                        //                                                                                                         //
////////////////////////////////////////////////////////////////////////////////////////////////////////////


#include <iostream>
#include <fstream>
#include <conio>
#include <math.h>
#include <numeric>
#include <values>
#include <vector>
#include <list>
#include <algorithm>
#include <numeric>
#include<iomanip.h>

using namespace std;


double a (double &t,double &teta,double &w); // a is the angular accelration function

double  tau=0.01,T=200,ac,g=9.8,L=g,m=1.,teta0=0.2,w0=0.,gamma=5.,q=gamma/(m*L),omega=0.67,A0=0.9,
        kteta1,kteta2,lteta1,lteta2,tmid,tetamid,wmid,Anumeric=0.,omegamax=4.,delomega=0.1;

// teta0=initial deflection angle, w0=initial angular velocity, tau=timestep; T=simulation time,
// E=total energy, L=pendulum length, m=pendulum mass, gamma=damping coefficient,
// omega= angular frequency of driving force, A0=sinusoidal driving force amplitude,


int N=(T/tau), Nomega=int(omegamax/delomega); // N=number of time step

main()

{


vector<double> w(N+1,0),teta(N+1,0),Acomp(Nomega,0);

// Arrays teta and w store the deflection angle and angular velocity at each time step. Array Acomp
// stores the pendulum amplitude for a given angular frequency of the sinusoidal driving force. 

ofstream tetaER("tetaER A0=0.9 free.plt"); // output file of pendulum angle (ER algorithm).
ofstream wER("wER.plt"); // output file of pendulum angular velocity (ER algorithm).
ofstream tetaRK2("tetaRK2.plt"); // output file of pendulum angle (RK2 algorithm).
ofstream wRK2("wRK2.plt"); // output file of pendulum angular velocity (RK2 algorithm).
ofstream phaseER("phase-space ER.plt"); // output file for pendulum phase space plot
ofstream Ampcomp("Acomp-w gamma=0.5.plt");  //  output file for amplitude vs omega


//for(int s=0;s<Nomega;s++){ cout<<"s= "<<s<<endl;//getch();

//omega+=delomega;

//------------------------ Euler-Richardson Method------------------------

teta[0]=teta0; w[0]=w0; Anumeric=0;

for(int t=0;t<N;t++){

tmid=t*tau + 0.5*tau;
tetamid=teta[t] + 0.5*w[t]*tau;
wmid=w[t] + 0.5*tau*a(t*tau,teta[t],w[t]);

w[t+1]=w[t] + tau*a(tmid,tetamid,wmid);
teta[t+1]=teta[t] + tau*wmid;

//if (teta[t+1]>M_PI)  teta[t+1]-=M_PI;
//if (teta[t+1]<-M_PI) teta[t+1]+=M_PI;


tetaER<<t*tau<<" "<<teta[t]<<endl;
wER<<t*tau<<" "<<w[t]<<endl;
phaseER<<teta[t]<<"  "<<w[t]<<endl;

if (t*tau>(10/q) && t*tau<(10/q) + 2*2*(M_PI/omega) && teta[t]>0 && teta[t+1]>teta[t]) Anumeric=teta[t+1];


}//end t

Ampcomp<<omega<<" "<<Anumeric<<endl;

//cout<<"Anumeric ER= "<<Anumeric<<endl;


//------------------------- Runge-Kutta 2nd order Method ------------------------

teta[0]=teta0; w[0]=w0;

for(int t=0;t<N;t++){

kteta1=tau*w[t];
lteta1=tau*a(t*tau,teta[t],w[t]);

kteta2=tau*(w[t]+lteta1);
lteta2=tau*a(t*tau+tau,teta[t]+kteta1,w[t]+lteta1);

w[t+1]=w[t]+0.5*(lteta1+lteta2);

teta[t+1]=teta[t]+0.5*(kteta1+kteta2);

if (teta[t+1]>M_PI)  teta[t+1]-=M_PI;
if (teta[t+1]<-M_PI) teta[t+1]+=M_PI;


tetaRK2<<t*tau<<" "<<teta[t]<<endl;
wRK2<<t*tau<<" "<<w[t]<<endl;

if (t*tau>(10/q) && t*tau<(10/q) + 2*(M_PI/omega) && teta[t]>0 && teta[t+1]>teta[t])  Anumeric=teta[t+1];

}//end t

//Ampcomp<<omega<<" "<<Anumeric<<endl;

//cout<<"Anumeric RK2= "<<Anumeric<<endl;getch();

//}// end s

cout<<"end."<<endl;getch();

return 0;

}

//----------------------------------------------------------------

double a (double &t,double &teta,double &w) {

return ac=-(g/L)*sin(teta) - q*w + A0*cos(omega*t);

}